#ifndef OMPL_GEOMETRIC_PLANNERS_TRRT_TRRT_H
#define OMPL_GEOMETRIC_PLANNERS_TRRT_TRRT_H

#include <ompl/base/Planner.h>
#include <ompl/datastructures/NearestNeighbors.h>
#include <ompl/geometric/planners/PlannerIncludes.h>
#include <ompl_extension/CostEstimatorBase.h>

namespace ompl
{
namespace geometric
{

class TRRT : public base::Planner
{
public:
  TRRT(const base::SpaceInformationPtr& si) : base::Planner(si,"TRRT")
  {
    specs_.approximateSolutions = true;

    maxDistance_ = 0.0;

    temperature_ = 1e-6; // initial temperature

    temperatureFactor_ = 2; // default temperature change ratio

    numFailMax_ = 10; // default max number of failures

    normalizeK_ = 1; // initial value of normalize factor K

    costMax_ = std::numeric_limits<double>::max();

    goalBias_ = 0.05;
  }

  virtual ~TRRT()
  {
    freeMemory();
  }

  virtual void clear();

  virtual bool solve(const base::PlannerTerminationCondition& ptc);

  virtual void getPlannerData(base::PlannerData& data) const;

  void setGoalBias(double goalBias)
  {
      goalBias_ = goalBias;
  }

  /** \brief Get the goal bias the planner is using */
  double getGoalBias(void) const
  {
      return goalBias_;
  }

  double getRange() const
  {
    return maxDistance_;
  }

  void setRange(double distance)
  {
    maxDistance_ = distance;
  }

  double getTemperature() const
  {
    return temperature_;
  }

  void setTemperature(double temperature)
  {
    temperature_ = temperature;
  }

  double getTemperatureFactor() const
  {
    return temperatureFactor_;
  }

  void setTemperatureFactor(double temperatureFactor)
  {
    temperatureFactor_ = temperatureFactor;
  }

  double getNumFailMax() const
  {
    return numFailMax_;
  }

  void setNumFailMax(double numFailMax)
  {
    numFailMax_ = numFailMax;
  }

  double getNormalizeK() const
  {
    return normalizeK_;
  }

  void setNormalizeK(double normalizeK)
  {
    normalizeK_ = normalizeK;
  }

  double getCostMax() const
  {
    return costMax_;
  }

  void setCostMax(double costMax)
  {
    costMax_ = costMax;
  }

  void setMotionCostEstimator(base::MotionCostEstimator* estimator)
  {
    motionCostEstimator_.reset(estimator);
  }

  base::MotionCostEstimatorPtr getMotionCostEstimator() const
  {
    return motionCostEstimator_;
  }


  void setStateCostEstimator(base::StateCostEstimator* estimator)
  {
    stateCostEstimator_.reset(estimator);
  }

  base::StateCostEstimatorPtr getStateCostEstimator() const
  {
    return stateCostEstimator_;
  }

  template<template<typename T> class NN>
  void setNearestNeighbors()
  {
    nn_.reset(new NN<Motion*>());
  }

  virtual void setup();


protected:

  class Motion
  {
  public:
    Motion() : state(NULL), parent(NULL), cost(0.0) {}
    Motion(const base::SpaceInformationPtr& si) : state(si->allocState()), parent(NULL), cost(0.0)
    {
    }

    ~Motion() {}

    base::State* state;

    Motion* parent;

    double cost;
  };

  void freeMemory();

  double distanceFunction(const Motion* a, const Motion* b) const
  {
    return si_->distance(a->state, b->state);
  }

  double goalBias_;

  base::StateSamplerPtr sampler_;

  boost::shared_ptr<NearestNeighbors<Motion*> > nn_;

  double maxDistance_;

  RNG rng_;

  double temperature_;

  unsigned int numFailMax_;

  double normalizeK_;

  double temperatureFactor_;

  double costMax_;

  base::MotionCostEstimatorPtr motionCostEstimator_;

  base::StateCostEstimatorPtr stateCostEstimator_;
};


}
}

#endif
